import lejos.nxt.SensorPort;

public class MowController extends Thread {
	private int lanesDone = 0;
	private MotorController motorCtrl;
	private MowUnitController mowUnitCtrl;
	private boolean inField = false;
	private boolean straight, straighteningInProgress;
	
	public MowController() {
		setName("MowController");
		setPriority(MIN_PRIORITY);
		
		motorCtrl = new MotorController(Configuration.MOTOR_LEFT, Configuration.MOTOR_RIGHT);
		mowUnitCtrl = new MowUnitController();
		mowUnitCtrl.calibrate();
	}
	
	public void run() {
		while (!Thread.interrupted()) {
			try {
				Thread.sleep(10000);
			} catch (InterruptedException e) {
				break;
			}
		}
	}
	
	private boolean straightenUp(boolean rightSideDone) {
		DebugLogger.write("straightenUp");
		if (rightSideDone) {
			motorCtrl.flt(motorCtrl.RIGHT);
		} else {
			motorCtrl.flt(motorCtrl.LEFT);
		}
		
		straighteningInProgress = !straighteningInProgress;
		
		DebugLogger.write("straightenUp done");
		return !straighteningInProgress;
	}
	
	public synchronized void lineReached(SensorPort port) {
		DebugLogger.write("Line found. Sensor: " + (port == Configuration.COLOR_LIGHTSENSOR_PORT ? "Color" : "Light"), true);
		// If preline is set, detecting a line should start the bot.
		if (!inField) {
			if (!straight) {
				DebugLogger.write("!inField, !straight");
				straight = straightenUp(port == Configuration.RIGHT_LIGHTSENSOR_PORT);
			}
			if (straight) {
				straight = false;
				
				DebugLogger.write("Out field.Start!");
				motorCtrl.stop();
				motorCtrl.driveDistance(Configuration.MOW_UNIT_LENGTH);
				inField = true;
				mowUnitCtrl.down();
				motorCtrl.resetTacho();
				motorCtrl.forward();
			}
		}
		// Check if end of lane is possible by comparing distance.
		else if (Configuration.LANE_LENGTH - motorCtrl.getTravelledDistance() < Configuration.LANE_LENGTH_ERROR_MARGIN) {
			if (!straight) {
				DebugLogger.write("!straight");
				straight = straightenUp(port == Configuration.RIGHT_LIGHTSENSOR_PORT);
			}
			if (straight) {
				straight = false;
				DebugLogger.write("End lane", true);
				lanesDone++;
				motorCtrl.stop();
				mowUnitCtrl.up();
				
				if (lanesDone == 7) {
					mowUnitCtrl.down();
					motorCtrl.driveDistance(Configuration.WHEEL_TO_LINESENSOR);
					laatsteRondje();
					stop();
				}
				
				motorCtrl.driveDistance(2*Configuration.WHEEL_TO_LINESENSOR + Configuration.AFTER_LINE_CORRECTION);
				motorCtrl.stop();
				
				// Make a turn.
				DebugLogger.write("Turning", true);
				if (Configuration.FIRST_TURN == lanesDone%2) {
					// Turn right
					motorCtrl.drive(100, 0.5*Configuration.LANE_WIDTH, 0.5);
				} else {
					// Turn left
					motorCtrl.drive(100, -0.5*Configuration.LANE_WIDTH, 0.5);
				}
				inField = false;
				motorCtrl.stop();
				DebugLogger.write("Forward");
				motorCtrl.forward();
			}
		}
	}
	
	public void laatsteRondje() {
		int draaiHoek = -90;
		int bochtSnelheid = 150;
		int rechtSnelheid = 300;
		int direction = (1 - lanesDone%2 * 2);
		draaiHoek *= direction;
		
		
		//als het startpunt rechtsonder is, dan moet de robot naar links draaien (negatief)
		//als het startpunt linksonder is, dan moet de robot naar rechts(positief)
		
		motorCtrl.turn(draaiHoek, bochtSnelheid);
		
		motorCtrl.drive(rechtSnelheid, rechtSnelheid, 1100, 1100);
		motorCtrl.stop();
		motorCtrl.turn(draaiHoek, bochtSnelheid);
		
		motorCtrl.drive(rechtSnelheid, rechtSnelheid, 2200, 2200);
		motorCtrl.stop();
		motorCtrl.turn(draaiHoek, bochtSnelheid);
		
		motorCtrl.drive(rechtSnelheid, rechtSnelheid, 1100, 1100);
		motorCtrl.stop();
		
		stop();
	}

	public void stop() {
		DebugLogger.write("Stop", true);
		DebugLogger.close();
		interrupt();
	}
	public void resume() {
		DebugLogger.write("Resume", true);
		motorCtrl.forward();
	}
	public void pause() {
		DebugLogger.write("Pause", true);
		motorCtrl.stop();
	}
}
